using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.Core.DsspHttp;
using Microsoft.Dss.Core.DsspHttpUtilities;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Robotics.PhysicalModel.Proxy;

using System;
using System.Net;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Collections.Specialized;
using System.ComponentModel;
using System.Security.Permissions;
using xml = System.Xml;
using W3C.Soap;

using sonar = Microsoft.Robotics.Services.Sonar.Proxy;
using vex = VEXRobotics.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;


namespace VEXService.Sonar
{
    /// <summary>
    /// Define the operations for the service
    /// </summary>
    [ServicePort]
    public class VEXSonarOperations : PortSet<
        DsspDefaultLookup,
        DsspDefaultDrop,
        Get,
        sonar.Replace,
        sonar.ReliableSubscribe,
        sonar.Subscribe,
        HttpGet,
        HttpPost>
    {
    }


    /// <summary>
    /// Get the state
    /// </summary>
    [DataContract]
    public class Get : Get<GetRequestType, PortSet<VEXSonarState, Fault>>
    {
    }

    /// <summary>
    /// VEX Sonar Contract 
    /// </summary>
    public sealed class Contract
    {
        public const string Identifier = "http://schemas.vexrobotics.com/2011/09/vexservice.sonar.html";
    }

    [Contract(Contract.Identifier)]
    [AlternateContract(sonar.Contract.Identifier)]
    [DisplayName("VEX Sonar")]
    [Description("Provides access to the VEX sonar sensor.")]
    [DssServiceDescription("http://msdn.microsoft.com/en-us/library/dd145256.aspx")]
    [DssCategory(vex.VEXCategories.VEX)]
    public class VEXSonarService : DsspServiceBase
    {
        /// <summary>
        /// XSLT Transform to use for display
        /// </summary>
        [EmbeddedResource("VEXService.Config.Sonar.xslt")]
        private string _transform = null;

        /// <summary>
        /// Sonar State
        /// </summary>
        [InitialStatePartner(Optional = true, ServiceUri = ServicePaths.Store + @"/VEX.Sonar.config.xml")]
        private VEXSonarState _State = null;

        /// <summary>
        /// Service port
        /// </summary>
        [ServicePort("/VEX/Sonar", AllowMultipleInstances = true)]
        private VEXSonarOperations _mainPort = new VEXSonarOperations();

        /// <summary>
        /// Alternate port for the generic port
        /// </summary>
        [AlternateServicePort("/sonar",
            AllowMultipleInstances = true,
            AlternateContract = sonar.Contract.Identifier)]
        private sonar.SonarOperations _genericPort = new sonar.SonarOperations();

        /// <summary>
        /// Port to talk with the VEX controller
        /// </summary>
        [Partner("VEX", Contract = vex.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate,
            Optional = false)]
        private vex.VEXOperations _vexPort = new vex.VEXOperations();
        vex.VEXOperations _notificationPort = new vex.VEXOperations();

        /// <summary>
        /// HTTP helper
        /// </summary>
        DsspHttpUtilitiesPort _httpUtilities = new DsspHttpUtilitiesPort();

        /// <summary>
        /// Subscription manager
        /// </summary>
        [Partner("SubMgr", Contract = submgr.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.CreateAlways,
            Optional = false)]
        private submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="creationPort"></param>
        public VEXSonarService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }

        #region Initialization Code

        /// <summary>
        /// Startup and initialize the service
        /// </summary>
        protected override void Start()
        {
            LogVerbose(LogGroups.Console, "Configuring VEX Sonar Sensor... ");

            #region Initialize Service State
            if (_State == null)
            {
                _State = new VEXSonarState();
                SaveState(_State);
            }
            #endregion

            base.Start();

            #region Initialize Service State
            // Reset the controller to know about our configuration
            Port<bool> donePort = new Port<bool>();
            SpawnIterator<Port<bool>>(donePort, InitializeKnownConnection);
            Activate(Arbiter.Receive(false, donePort,
                delegate(bool success)
                {
                    if (success)
                    {
                        // display HTTP service Uri
                        LogInfo(LogGroups.Console, "Service uri: ");
                    }
                    else
                    {
                        LogError(LogGroups.Console, "VEX Sonar service failed to start.");
                        _mainPort.Post(new DsspDefaultDrop());
                    }
                }));
            #endregion

            // Needed for HttpPost
            _httpUtilities = DsspHttpUtilitiesService.Create(Environment);
        }

        /// <summary>
        /// Set a valid sonar sensor to hook up to
        /// </summary>
        /// <returns></returns>
        private IEnumerator<ITask> InitializeKnownConnection(Port<bool> resultPort)
        {
            bool serviceSuccess = true;

            // Configure default state
            if (_State.PortIdentifier == vex.SensorPort.SensorPort_None ||
                _State.InterruptPort == vex.InterruptPort.InterruptPort_None)
            {
                LogError(_State.Name + ": No sensor/interrupt port selected");
                serviceSuccess = false;
            }
            else
            {
                vex.SelectiveInterruptConfigure intConfig = new vex.SelectiveInterruptConfigure();
                intConfig.Body.Interrupt = (vex.InterruptPort)_State.InterruptPort;
                intConfig.Body.Port = _State.PortIdentifier;
                intConfig.Body.Type = vex.SensorDefinition.SensorType.Sonar;

                _vexPort.Post(intConfig);
                yield return Arbiter.Choice(intConfig.ResponsePort,
                    delegate(DefaultReplaceResponseType response)
                    {
                    },
                    delegate(Fault fault)
                    {
                        serviceSuccess = false;
                        LogError(LogGroups.Console, "Failed to update VEX service configuration", fault);
                    });

                // Subscribe to VEX for notifications
                if (serviceSuccess)
                {
                    serviceSuccess = false;
                    SaveState(_State);
                    Port<bool> subscribeDonePort = new Port<bool>();
                    SpawnIterator<Port<bool>>(subscribeDonePort, SubscribeToVEX);
                    yield return (Arbiter.Receive<bool>(false, subscribeDonePort,
                        delegate(bool success)
                        {
                            serviceSuccess = success;
                        }));
                }
            }

            resultPort.Post(serviceSuccess);
            yield break;
        }

        /// <summary>
        /// Subscribe to appropriate sensor type and port on VEX
        /// </summary>
        private IEnumerator<ITask> SubscribeToVEX(Port<bool> successPort)
        {
            // Create a custom subscription request
            vex.CustomSubscribeRequestType request = new vex.CustomSubscribeRequestType();

            // Select only the sensor and port we want
            // NOTE: this name must match the VEX sensor name.
            request.Sensors = new List<vex.SensorDefinition>();
            vex.SensorDefinition sensor = new vex.SensorDefinition();
            sensor.Type = vex.SensorDefinition.SensorType.Sonar;
            sensor.Port = (vex.SensorPort) _State.InterruptPort;
            request.Sensors.Add(sensor);

            // Subscribe to the VEX and wait for a response
            yield return
                Arbiter.Choice(_vexPort.SelectiveSubscribe(request, _notificationPort),
                    delegate(SubscribeResponseType Rsp)
                    {
                        //Subscription was successful, start listening for sensor change notifications
                        Activate(
                            Arbiter.Receive<vex.Update>(true, _notificationPort, SensorNotificationHandler)
                        );

                        // Update our state with subscription status
                        successPort.Post(true);
                    },
                    delegate(Fault fault)
                    {
                        LogError("Sonar subscription failed", fault);
                        successPort.Post(false);
                    }
                );

            yield break;
        }
        #endregion

        /// <summary>
        /// Handle sensor update message from VEX
        /// </summary>
        private void SensorNotificationHandler(vex.Update notify)
        {
            if (_State.InterruptPort != vex.InterruptPort.InterruptPort_None &&
                _State.PortIdentifier != vex.SensorPort.SensorPort_None)
            {
                // Update time
                _State.TimeStamp = DateTime.Now;

                // Update reading
                /*
                 * Scale the reading based upon measurements
                 * Distance	Reading
                 *  (cm)    (ticks)
                 *    3	     3
                 *   10	     6
                 *   20	    12
                 *   30	    17
                 *   40	    23
                 *   50	    28
                 *   60	    33
                 *   80	    45
                 *   
                 * Based upon the graph, we will ignore the first and last
                 * measurements.  This means that the slope is:
                 * (60-10)/(33-6) = 1.85185 cm/Tick
                 * 
                 * The zero intercept is:
                 * 30 = 1.85185 * 17 + Z
                 * Z = -1.48
                 */
                if (UInt32.MaxValue != notify.Body.InterruptSensors.Sensors[(int)_State.InterruptPort - 1].Value)
                {
                    _State.DistanceMeasurement = notify.Body.InterruptSensors.Sensors[(int)_State.InterruptPort - 1].Value;
                    _State.DistanceMeasurement = (_State.DistanceMeasurement * 1.85185) - 1.48;
                    _State.DistanceMeasurement /= 100.0;    // cm to meters
                }
                else
                {
                    _State.DistanceMeasurement = Double.NaN;
                }

                // Notify subscribers on any state change
                sonar.SonarState genericState = new sonar.SonarState();
                SendNotification<sonar.Replace>(_subMgrPort, new sonar.Replace(_State.ToGenericState(genericState)));
            }
        }

        /// <summary>
        /// Get the state
        /// </summary>
        /// <param name="get">Response port to post the state</param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_State);
            yield break;
        }

        /// <summary>
        /// Get the state
        /// </summary>
        /// <param name="get">Response port to post the state</param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> GenericGetHandler(sonar.Get get)
        {
            sonar.SonarState genericState = new sonar.SonarState();
            get.ResponsePort.Post(_State.ToGenericState(genericState));
            yield break;
        }

        /// <summary>
        /// Get the state to HTTP display
        /// </summary>
        /// <param name="httpGet">Response port to post the state</param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> HttpGetHandler(HttpGet httpGet)
        {
            httpGet.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _State,
                _transform)
            );
            yield break;
        }


        /// <summary>
        /// HttpPost Handler
        /// </summary>
        /// <param name="submit"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> HttpPostHandler(HttpPost httpPost)
        {
            // Use helper to read form data
            ReadFormData readForm = new ReadFormData(httpPost);
            _httpUtilities.Post(readForm);

            // Wait for result
            Activate(Arbiter.Choice(readForm.ResultPort,
                delegate(NameValueCollection parameters)
                {
                    if (parameters["Name"] != null)
                    {
                        // Update our state variables
                        _State.Name = parameters["Name"];
                    }
                    HttpPostSuccess(httpPost);
                },
                delegate(Exception fault)
                {
                    LogError(fault.Message);
                    HttpPostFailure(httpPost, fault.Message);
                })
            );
            yield break;
        }

        /// <summary>
        /// Send Http Post Success Response
        /// </summary>
        /// <param name="httpPost"></param>
        private void HttpPostSuccess(HttpPost httpPost)
        {
            HttpResponseType rsp =
                new HttpResponseType(HttpStatusCode.OK, _State, _transform);
            httpPost.ResponsePort.Post(rsp);
        }

        /// <summary>
        /// Send Http Post Failure Response
        /// </summary>
        /// <param name="httpPost"></param>
        /// <param name="failureReason"></param>
        private static void HttpPostFailure(HttpPost httpPost, string failureReason)
        {
            HttpResponseType rsp =
                new HttpResponseType(HttpStatusCode.ExpectationFailed, failureReason);
            httpPost.ResponsePort.Post(rsp);
        }
        
        /// <summary>
        /// Replace the state with the given update
        /// </summary>
        /// <param name="replace">Replacement state</param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> ReplaceHandler(sonar.Replace replace)
        {
            if ((int)_State.InterruptPort != replace.Body.HardwareIdentifier)
            {
                Fault fault = Fault.FromException(new InvalidOperationException("Invalid number of sensors"));
                replace.ResponsePort.Post(fault);
                yield break;
            }

            _State.FromGenericState(replace.Body);
            SaveState(_State);

            sonar.SonarState genericState = new sonar.SonarState();
            SendNotification<sonar.Replace>(_subMgrPort, _State.ToGenericState(genericState));
            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);

            yield break;
        }

        /// <summary>
        /// Subscribe to updates
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> SubscribeHandler(sonar.Subscribe subscribe)
        {
            yield return Arbiter.Choice(
                SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                delegate(SuccessResult success)
                {
                    sonar.SonarState genericState = new sonar.SonarState();
                    SendNotification<sonar.Replace>(_subMgrPort, subscribe.Body.Subscriber, new sonar.Replace(_State.ToGenericState(genericState)));
                },
                delegate(Exception fault)
                {
                    LogError(fault);
                }
            );
        }

        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> ReliableSubscribeHandler(sonar.ReliableSubscribe subscribe)
        {
            yield return Arbiter.Choice(
                SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                delegate(SuccessResult success)
                {
                    sonar.SonarState genericState = new sonar.SonarState();
                    SendNotification<sonar.Replace>(_subMgrPort, subscribe.Body.Subscriber, new sonar.Replace(_State.ToGenericState(genericState)));
                },
                delegate(Exception fault)
                {
                    LogError(fault);
                }
            );
        }
    }

    /// <summary>
    /// Define the state which is the port used
    /// </summary>
    [DataContract]
    [Description("Specifies the VEX Sonar state.")]
    public class VEXSonarState : IDssSerializable, ICloneable
    {
        private double _DistanceMeasurement;
        private string _Name;
        private Pose _Pose;
        private DateTime _TimeStamp;
        private vex.InterruptPort _InterruptPort;
        private vex.SensorPort _portIdentifier;

        public VEXSonarState()
        {
            _Pose = new Pose();
            _TimeStamp = DateTime.MinValue;
        }

        //
        // Summary:
        //     sensor port identifier
        [Description("Identifies the interrupt port of the sonar sensor.")]
        [DataMember(Order = -1)]
        public vex.InterruptPort InterruptPort
        {
            get { return _InterruptPort; }
            set { _InterruptPort = value; }
        }

        //
        // Summary:
        //     Hardware port identifier
        [Description("Identifies the sensor port of the sonar sensor.")]
        [DataMember(Order = -1)]
        public vex.SensorPort PortIdentifier
        {
            get { return _portIdentifier; }
            set { _portIdentifier = value; }
        }

        [DataMember]
        [Description("Specifies the descriptive identifier for the sonar.")]
        public string Name
        {
            get { return _Name; }
            set { this._Name = value; }
        }

        //
        // Summary:
        //     The distance reading in meters.
        [Description("Identifies the distance reading in meters.")]
        [Browsable(false)]
        [DataMember(Order = -1)]
        public double DistanceMeasurement
        {
            get { return _DistanceMeasurement; }
            set { _DistanceMeasurement = value; }
        }

        //
        // Summary:
        //     Position and orientation
        [Description("The position and orientation of the sonar sensor.")]
        [DataMember(Order = -1)]
        public Pose Pose
        {
            get { return _Pose; }
            set { _Pose = value; }
        }

        //
        // Summary:
        //     Timestamp of this sample
        [DefaultValue(typeof(DateTime), "0001-01-01T00:00:00")]
        [Browsable(false)]
        [DataMember(Order = -1, XmlOmitDefaultValue = true)]
        [Description("Identifies the timestamp for the reading of the sonar sensor.")]
        public DateTime TimeStamp
        {
            get { return _TimeStamp; }
            set { _TimeStamp = value; }
        }

        public void FromGenericState(sonar.SonarState genericState)
        {
            this.DistanceMeasurement = genericState.DistanceMeasurement;
            this.Pose = genericState.Pose;
            this.TimeStamp = genericState.TimeStamp;
        }

        public sonar.SonarState ToGenericState(sonar.SonarState genericState)
        {
            genericState.AngularRange = 0.34906585039886591538473815369772; // 20 degrees
            genericState.AngularResolution = 0.34906585039886591538473815369772; // 20 degrees
            genericState.DistanceMeasurement = this.DistanceMeasurement;
            genericState.DistanceMeasurements = new double[1];
                genericState.DistanceMeasurements[0] = this.DistanceMeasurement;
            genericState.HardwareIdentifier = (int)this.InterruptPort;
            genericState.MaxDistance = 2.5; // Meters
            genericState.Pose = this.Pose;
            genericState.TimeStamp = this.TimeStamp;

            return genericState;
        }

        #region IDssSerializable

        #endregion

        #region IDssSerializable

        /// <summary>
        /// Copy To sonar state
        /// </summary>
        public virtual void CopyTo(IDssSerializable target)
        {
            VEXSonarState typedTarget = target as VEXSonarState;

            if (typedTarget == null)
                throw new ArgumentException("CopyTo({0}) requires type {0}", this.GetType().FullName);

            typedTarget.DistanceMeasurement = this.DistanceMeasurement;
            typedTarget.Pose = this.Pose;
            typedTarget.Name = this.Name;
            typedTarget.TimeStamp = this.TimeStamp;
            typedTarget.InterruptPort = this.InterruptPort;
            typedTarget.PortIdentifier = this.PortIdentifier;

        }

        /// <summary>
        /// Clone the VEX Sonar State
        /// </summary>
        /// <returns></returns>
        public object Clone()
        {
            VEXSonarState clone = new VEXSonarState();
            clone.DistanceMeasurement = this.DistanceMeasurement;
            clone.InterruptPort = this.InterruptPort;
            clone.Name = this.Name;
            clone.PortIdentifier = this.PortIdentifier;
            clone.Pose = this.Pose;
            clone.TimeStamp = this.TimeStamp;

            return clone;
        }

        /// <summary>
        /// Serialize
        /// </summary>
        public virtual void Serialize(System.IO.BinaryWriter writer)
        {
            writer.Write(DistanceMeasurement);
            writer.Write(Name);
            Pose.Serialize(writer);
            Microsoft.Dss.Services.Serializer.BinarySerializationHelper.SerializeDateTime(TimeStamp, writer);
            writer.Write((int)InterruptPort);
            writer.Write((int)PortIdentifier);
        }

        /// <summary>
        /// Deserialize
        /// </summary>
        public virtual object Deserialize(System.IO.BinaryReader reader)
        {
            DistanceMeasurement = reader.ReadDouble();
            Name = reader.ReadString();
            Pose.Deserialize(reader);
            TimeStamp = Microsoft.Dss.Services.Serializer.BinarySerializationHelper.DeserializeDateTime(reader);
            InterruptPort = (vex.InterruptPort)reader.ReadInt32();
            PortIdentifier = (vex.SensorPort)reader.ReadInt32();

            return this;
        }

        #endregion
    };
}
